/**
 * SPI0 info:
    - SPI0_SCK: GPIO2_25(Pin 89, wPi 14), number 23
    - SPI0_MISO: GPIO2_28(Pin 92, wPi 12), number 21
    - SPI0_MOSI:GPIO2_27(Pin 91, wPi 11), number 19
    - SPI0_CS: GPIO2_26(Pin 90, wPi 15), number 24
    - linux device: /dev/spidev0.0
  * SPI_Slave_notiy:
    - GPIO2_15: wiringOP 8, Pin 79 
  * framework bug:
    - wiringOP/wiringPi/wiringPi.c: wiringPiISR
        -  sprintf (pinS, "%d", pin) ->  sprintf (pinS, "%d", bcmGpioPin) 
 */

#include <cstring>
#include <cstdio>
#include <stdint.h>
#include <unistd.h>

#include <linux/spi/spidev.h>

#include <wiringPi.h>
#include "wiringPiSPIFix.h"

#include <opencv2/opencv.hpp>

// spi config
#define PREFIX "RX"
#define SPI_SLAVE_NOTIFY_PIN 8
#define SPI_BUFF_LEN 256

// data buffer
static unsigned char txBuffer[SPI_BUFF_LEN], rxBuffer[SPI_BUFF_LEN];

// recv image buffer
std::vector<std::vector<unsigned char>> rxStack;
std::vector<unsigned char> imageArray;

static void hex_dump(const unsigned char *src, size_t length, size_t line_size, char *prefix)
{
	int i = 0;
	const unsigned char *address = src;
	const unsigned char *line = address;
	unsigned char c;

	printf("%s | ", prefix);
	while (length-- > 0) {
		printf("%02X ", *address++);
		if (!(++i % line_size) || (length == 0 && i % line_size)) {
			if (length == 0) {
				while (i++ % line_size)
					printf("__ ");
			}
			printf(" |");
			while (line < address) {
				c = *line++;
				printf("%c", (c < 32 || c > 126) ? '.' : c);
			}
			printf("|\n");
			if (length > 0)
				printf("%s | ", prefix);
		}
	}
}

void slaveInterrupt(void){
    // send
    usleep(2 * 1000); // 2ms
    int ret = wiringPiSPIDataRW(0, 0, rxBuffer, 64);
    if(ret < 0){
        printf("Spi data read failed\n");
    }else{
        if((int)rxBuffer[5] != 0){
            // copy
            std::vector<unsigned char> copyBuffer(rxBuffer, rxBuffer + 64);
            rxStack.push_back(copyBuffer);
        }

        hex_dump(rxBuffer, 64, 64, PREFIX);
    }
}


int main(){
    // init send buffer cache, and clean rxbuffer
    memset(txBuffer, 0x00, sizeof(txBuffer));
    memset(rxBuffer, 0xff, sizeof(rxBuffer));

    // setup
    wiringPiSetup();

    // init io with low level, wait pico to control signal
    pinMode(SPI_SLAVE_NOTIFY_PIN, INPUT);
    digitalWrite(SPI_SLAVE_NOTIFY_PIN, LOW);

    // init isr
    wiringPiISR(SPI_SLAVE_NOTIFY_PIN, INT_EDGE_FALLING, &slaveInterrupt);

    // init spi
    int ret = -1;
    uint32_t mode = 0;
    ret = wiringPiSPISetupMode (0, 0, 500 * 1000, mode);
    if(ret < 0){
        printf("Spi init failed\n");
        return -1;
    }

    printf("Start event loop\n");
    while (true)
    {
        if(rxStack.size() > 0){
            std::vector<unsigned char> data = rxStack.front();
            
            // decode
            int pack_number = 0;
            for(size_t i = 0; i < 4; i++){
                pack_number += (int)data[i] << (8 * i);
            }
            bool is_finish = data[4];
            int byte_len = data[5];
            printf("Now Package is: %d, is_finish: %d, byte_len: %d\n", pack_number, is_finish, byte_len);

            // push to data array
            //imageArray.insert(imageArray.end(), data[6], data[6 + byte_len]);
            for(size_t d = 0; d < byte_len; d++){
                imageArray.push_back(data[6 + d]);
                printf("%d ", data[6 + d]);
            }
            printf("\n");

            // if is finish
            if(is_finish == 1){
                cv::Mat mat(20, 20, CV_8UC3, imageArray.data());
                cv::imwrite("recv.jpg", mat);
            }
            rxStack.erase(rxStack.begin());
        }else{
            delay(10);
        }
    }
    
}